
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_mission.h
  * @author     baiyang
  * @date       2022-5-12
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include <gcs_mavlink/gcs.h>
#include <common/gp_defines.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
#include <storage_manager/storage_manager.h>
/*-----------------------------------macro------------------------------------*/
// definitions
#define AP_MISSION_EEPROM_VERSION           0x65AE  // version number stored in first four bytes of eeprom.  increment this by one when eeprom format is changed
#define AP_MISSION_EEPROM_COMMAND_SIZE      15      // size in bytes of all mission commands

#ifndef AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS
#define AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS 100     // allow up to 100 do-jump commands
#endif

#define AP_MISSION_JUMP_REPEAT_FOREVER      -1      // when do-jump command's repeat count is -1 this means endless repeat

#define AP_MISSION_CMD_ID_NONE              0       // mavlink cmd id of zero means invalid or missing command
#define AP_MISSION_CMD_INDEX_NONE           65535   // command index of 65535 means invalid or missing command
#define AP_MISSION_JUMP_TIMES_MAX           32767   // maximum number of times a jump can be executed.  Used when jump tracking fails (i.e. when too many jumps in mission)

#define AP_MISSION_FIRST_REAL_COMMAND       1       // command #0 reserved to hold home position

#define AP_MISSION_RESTART_DEFAULT          0       // resume the mission from the last command run by default

#define AP_MISSION_OPTIONS_DEFAULT          0       // Do not clear the mission when rebooting
#define AP_MISSION_MASK_MISSION_CLEAR       (1<<0)  // If set then Clear the mission on boot
#define AP_MISSION_MASK_DIST_TO_LAND_CALC   (1<<1)  // Allow distance to best landing calculation to be run on failsafe
#define AP_MISSION_MASK_CONTINUE_AFTER_LAND (1<<2)  // Allow mission to continue after land

#define AP_MISSION_MAX_WP_HISTORY           7       // The maximum number of previous wp commands that will be stored from the active missions history
#define LAST_WP_PASSED (AP_MISSION_MAX_WP_HISTORY-2)

/*----------------------------------typedef-----------------------------------*/
#pragma pack(1)

// jump command structure
struct Jump_Command {
    uint16_t target;        // target command id
    int16_t num_times;      // num times to repeat.  -1 = repeat forever
};

// condition delay command structure
struct Conditional_Delay_Command {
    float seconds;          // period of delay in seconds
};

// condition delay command structure
struct Conditional_Distance_Command {
    float meters;           // distance from next waypoint in meters
};

// condition yaw command structure
struct Yaw_Command {
    float angle_deg;        // target angle in degrees (0=north, 90=east)
    float turn_rate_dps;    // turn rate in degrees / second (0=use default)
    int8_t direction;       // -1 = ccw, +1 = cw
    uint8_t relative_angle; // 0 = absolute angle, 1 = relative angle
};

// change speed command structure
struct Change_Speed_Command {
    uint8_t speed_type;     // 0=airspeed, 1=ground speed
    float target_ms;        // target speed in m/s, -1 means no change
    float throttle_pct;     // throttle as a percentage (i.e. 1 ~ 100), 0 means no change
};

// set relay command structure
struct Set_Relay_Command {
    uint8_t num;            // relay number from 1 to 4
    uint8_t state;          // on = 3.3V or 5V (depending upon board), off = 0V.  only used for do-set-relay, not for do-repeat-relay
};

// repeat relay command structure
struct Repeat_Relay_Command {
    uint8_t num;            // relay number from 1 to 4
    int16_t repeat_count;   // number of times to trigger the relay
    float cycle_time;       // cycle time in seconds (the time between peaks or the time the relay is on and off for each cycle?)
};

// set servo command structure
struct Set_Servo_Command {
    uint8_t channel;        // servo channel
    uint16_t pwm;           // pwm value for servo
};

// repeat servo command structure
struct Repeat_Servo_Command {
    uint8_t channel;        // servo channel
    uint16_t pwm;           // pwm value for servo
    int16_t repeat_count;   // number of times to move the servo (returns to trim in between)
    float cycle_time;       // cycle time in seconds (the time between peaks or the time the servo is at the specified pwm value for each cycle?)
};

// mount control command structure
struct Mount_Control {
    float pitch;            // pitch angle in degrees
    float roll;             // roll angle in degrees
    float yaw;              // yaw angle (relative to vehicle heading) in degrees
};

// digicam control command structure
struct Digicam_Configure {
    uint8_t shooting_mode;  // ProgramAuto = 1, AV = 2, TV = 3, Man=4, IntelligentAuto=5, SuperiorAuto=6
    uint16_t shutter_speed;
    uint8_t aperture;       // F stop number * 10
    uint16_t ISO;           // 80, 100, 200, etc
    uint8_t exposure_type;
    uint8_t cmd_id;
    float engine_cutoff_time;   // seconds
};

// digicam control command structure
struct Digicam_Control {
    uint8_t session;        // 1 = on, 0 = off
    uint8_t zoom_pos;
    int8_t zoom_step;       // +1 = zoom in, -1 = zoom out
    uint8_t focus_lock;
    uint8_t shooting_cmd;
    uint8_t cmd_id;
};

// set cam trigger distance command structure
struct Cam_Trigg_Distance {
    float meters;           // distance
    uint8_t trigger;        // triggers one image capture immediately
};

// gripper command structure
struct Gripper_Command {
    uint8_t num;            // gripper number
    uint8_t action;         // action (0 = release, 1 = grab)
};

// AUX_FUNCTION command structure
struct AuxFunction {
    uint16_t function;  // from RC_Channel::AUX_FUNC
    uint8_t switchpos;  // from RC_Channel::AuxSwitchPos
};

// high altitude balloon altitude wait
struct Altitude_Wait {
    float altitude; // meters
    float descent_rate; // m/s
    uint8_t wiggle_time; // seconds
};

// nav guided command
struct Guided_Limits_Command {
    // max time is held in p1 field
    float alt_min;          // min alt below which the command will be aborted.  0 for no lower alt limit
    float alt_max;          // max alt above which the command will be aborted.  0 for no upper alt limit
    float horiz_max;        // max horizontal distance the vehicle can move before the command will be aborted.  0 for no horizontal limit
};

// do VTOL transition
struct Do_VTOL_Transition {
    uint8_t target_state;
};

// navigation delay command structure
struct Navigation_Delay_Command {
    float seconds; // period of delay in seconds
    int8_t hour_utc; // absolute time's hour (utc)
    int8_t min_utc; // absolute time's min (utc)
    int8_t sec_utc; // absolute time's sec (utc)
};

// DO_ENGINE_CONTROL support
struct Do_Engine_Control {
    bool start_control; // start or stop engine
    bool cold_start; // use cold start procedure
    uint16_t height_delay_cm; // height delay for start
};

// NAV_SET_YAW_SPEED support
struct Set_Yaw_Speed {
    float angle_deg;        // target angle in degrees (0=north, 90=east)
    float speed;            // speed in meters/second
    uint8_t relative_angle; // 0 = absolute angle, 1 = relative angle
};

// winch command structure
struct Winch_Command {
    uint8_t num;            // winch number
    uint8_t action;         // action (0 = relax, 1 = length control, 2 = rate control)
    float release_length;   // cable distance to unwind in meters, negative numbers to wind in cable
    float release_rate;     // release rate in meters/second
};

// Scripting command structure
struct scripting_Command {
    float p1;
    float p2;
    float p3;
};

// Scripting NAV command (with verify)
struct nav_script_time_Command {
    uint8_t command;
    uint8_t timeout_s;
    float arg1;
    float arg2;
};

// Scripting NAV command (with verify)
struct nav_attitude_time_Command {
    uint16_t time_sec;
    int16_t roll_deg;
    int8_t pitch_deg;
    int16_t yaw_deg;
    int16_t climb_rate;
};

// MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW support
struct gimbal_manager_pitchyaw_Command {
    int8_t pitch_angle_deg;
    int16_t yaw_angle_deg;
    int8_t pitch_rate_degs;
    int8_t yaw_rate_degs;
    uint8_t flags;
    uint8_t gimbal_id;
};

#pragma pack()

union Mission_Content {
    // jump structure
    struct Jump_Command jump;

    // conditional delay
    struct Conditional_Delay_Command delay;

    // conditional distance
    struct Conditional_Distance_Command distance;

    // conditional yaw
    struct Yaw_Command yaw;

    // change speed
    struct Change_Speed_Command speed;

    // do-set-relay
    struct Set_Relay_Command relay;

    // do-repeat-relay
    struct Repeat_Relay_Command repeat_relay;

    // do-set-servo
    struct Set_Servo_Command servo;

    // do-repeate-servo
    struct Repeat_Servo_Command repeat_servo;

    // mount control
    struct Mount_Control mount_control;

    // camera configure
    struct Digicam_Configure digicam_configure;

    // camera control
    struct Digicam_Control digicam_control;

    // cam trigg distance
    struct Cam_Trigg_Distance cam_trigg_dist;

    // do-gripper
    struct Gripper_Command gripper;

    // arbitrary aux function
    struct AuxFunction auxfunction;

    // do-guided-limits
    struct Guided_Limits_Command guided_limits;

    // high altitude balloon altitude wait
    struct Altitude_Wait altitude_wait;

    // do vtol transition
    struct Do_VTOL_Transition do_vtol_transition;

    // DO_ENGINE_CONTROL
    struct Do_Engine_Control do_engine_control;

    // navigation delay
    struct Navigation_Delay_Command nav_delay;

    // NAV_SET_YAW_SPEED support
    struct Set_Yaw_Speed set_yaw_speed;

    // do-winch
    struct Winch_Command winch;

    // do scripting
    struct scripting_Command scripting;

    // nav scripting
    struct nav_script_time_Command nav_script_time;

    // nav attitude time
    struct nav_attitude_time_Command nav_attitude_time;

    // MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW
    struct gimbal_manager_pitchyaw_Command gimbal_manager_pitchyaw;

    // location
    Location location;      // Waypoint location
};

// mission state enumeration
enum mission_state {
    MISSION_STOPPED=0,
    MISSION_RUNNING=1,
    MISSION_COMPLETE=2
};

typedef struct Mission_Command* mission_command_t;

// command structure
struct Mission_Command {
    uint16_t index;             // this commands position in the command list
    uint16_t id;                // mavlink command id
    uint16_t p1;                // general purpose parameter 1
    union Mission_Content content;

    // for items which store in location, we offer a few more bits
    // of storage:
    uint8_t type_specific_bits;  // bitmask of set/unset bits
};

typedef struct mb_mission* mb_mission_t;
typedef bool (*mission_cmd_fn_t)(const mission_command_t);
typedef void (*mission_complete_fn_t)(void);

// jump related variables
struct jump_tracking_struct {
    uint16_t index;                 // index of do-jump commands in mission
    int16_t num_times_run;          // number of times this jump command has been run
};

// command structure
struct mb_mission {
    struct StorageAccess _storage;

    struct Mission_Flags {
        enum mission_state state;
        bool nav_cmd_loaded;         // true if a "navigation" command has been loaded into _nav_cmd
        bool do_cmd_loaded;          // true if a "do"/"conditional" command has been loaded into _do_cmd
        bool do_cmd_all_done;        // true if all "do"/"conditional" commands have been completed (stops unnecessary searching through eeprom for do commands)
        bool in_landing_sequence;   // true if the mission has jumped to a landing
        bool resuming_mission;      // true if the mission is resuming and set false once the aircraft attains the interrupted WP
    } _flags;

    // mission WP resume history
    uint16_t _wp_index_history[AP_MISSION_MAX_WP_HISTORY]; // storing the nav_cmd index for the last 6 WPs

    // pointer to main program functions
    mission_cmd_fn_t        _cmd_start_fn;  // pointer to function which will be called when a new command is started
    mission_cmd_fn_t        _cmd_verify_fn; // pointer to function which will be called repeatedly to ensure a command is progressing
    mission_complete_fn_t   _mission_complete_fn;   // pointer to function which will be called when mission completes

    // parameters
    Param_int16                _cmd_total;  // total number of commands in the mission
    Param_int16                _options;    // bitmask options for missions, currently for mission clearing on reboot but can be expanded as required
    Param_int8                 _restart;   // controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run)

    // internal variables
    bool                    _force_resume;  // when set true it forces mission to resume irrespective of MIS_RESTART param.
    uint16_t                _repeat_dist; // Distance to repeat on mission resume (m), can be set with MAV_CMD_DO_SET_RESUME_REPEAT_DIST
    struct Mission_Command  _nav_cmd;   // current "navigation" command.  It's position in the command list is held in _nav_cmd.index
    struct Mission_Command  _do_cmd;    // current "do" command.  It's position in the command list is held in _do_cmd.index
    struct Mission_Command  _resume_cmd;  // virtual wp command that is used to resume mission if the mission needs to be rewound on resume.
    uint16_t                _prev_nav_cmd_id;       // id of the previous "navigation" command. (WAYPOINT, LOITER_TO_ALT, ect etc)
    uint16_t                _prev_nav_cmd_index;    // index of the previous "navigation" command.  Rarely used which is why we don't store the whole command
    uint16_t                _prev_nav_cmd_wp_index; // index of the previous "navigation" command that contains a waypoint.  Rarely used which is why we don't store the whole command
    Location                _exit_position;  // the position in the mission that the mission was exited

    // jump related variables
    struct jump_tracking_struct _jump_tracking[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS];

    // last time that mission changed
    uint32_t _last_change_time_ms;

    // memoisation of contains-relative:
    bool _contains_terrain_alt_items;  // true if the mission has terrain-relative items
    uint32_t _last_contains_relative_calculated_ms;  // will be equal to _last_change_time_ms if _contains_terrain_alt_items is up-to-date
};

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
mb_mission_t mission_get_singleton();

void mission_init(mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn);

/// update - ensures the command queues are loaded with the next command and calls main programs command_init and command_verify functions to progress the mission
///     should be called at 10hz or higher
void mission_update();

/// clear - clears out mission
///     returns true if mission was running so it could not be cleared
bool mission_clear();

/// trucate - truncate any mission items beyond index
void mission_truncate(uint16_t index);

/// start - resets current commands to point to the beginning of the mission
///     To-Do: should we validate the mission first and return true/false?
void mission_start();

/// stop - stops mission execution.  subsequent calls to update() will have no effect until the mission is started or resumed
void mission_stop();

/// resume - continues the mission execution from where we last left off
///     previous running commands will be re-initialized
void mission_resume();

/// check mission starts with a takeoff command
bool mission_starts_with_takeoff_cmd();

/*
    return true if MIS_OPTIONS is set to allow continue of mission
    logic after a land and the next waypoint is a takeoff. If this
    is false then after a landing is complete the vehicle should 
    disarm and mission logic should stop
*/
bool mission_continue_after_land_check_for_takeoff();
bool mission_continue_after_land(void);

/// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
void mission_start_or_resume();

/// reset - reset mission to the first command
void mission_reset();

// reset the mission history to prevent recalling previous mission histories after a mission restart.
void mission_reset_wp_history(void);

/// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
bool mission_is_nav_cmd(const mission_command_t cmd);

/// load_cmd_from_storage - load command from storage
///     true is return if successful
bool mission_read_cmd_from_storage(uint16_t index, mission_command_t  cmd);

/// write_cmd_to_storage - write a command to storage
///     index is used to calculate the storage location
///     true is returned if successful
bool mission_write_cmd_to_storage(uint16_t index, const mission_command_t cmd);

/// return total number of commands that can fit in storage space
uint16_t mission_num_commands_max(void);

// set_current_cmd - jumps to command specified by index
// bool rewind = false
bool mission_set_current_cmd(uint16_t index, bool rewind);

// restart current navigation command.  Used to handle external changes to mission
// returns true on success, false if mission is not running or current nav command is invalid
bool mission_restart_current_nav_cmd();

/// num_commands - returns total number of commands in the mission
///                 this number includes offset 0, the home location
uint16_t mission_num_commands();

// return the last time the mission changed in milliseconds
uint32_t mission_last_change_time_ms(void);

/// get_current_nav_index - returns the current "navigation" command index
/// Note that this will return 0 if there is no command. This is
/// used in MAVLink reporting of the mission command
uint16_t mission_get_current_nav_index();

/// get_next_nav_cmd - gets next "navigation" command found at or after start_index
///     returns true if found, false if not found (i.e. reached end of mission command list)
///     accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this)
bool mission_get_next_nav_cmd(uint16_t start_index, mission_command_t cmd);

/// add_cmd - adds a command to the end of the command list and writes to storage
///     returns true if successfully added, false on failure
///     cmd.index is updated with it's new position in the mission
bool mission_add_cmd(mission_command_t cmd);

/// replace_cmd - replaces the command at position 'index' in the command list with the provided cmd
///     replacing the current active command will have no effect until the command is restarted
///     returns true if successfully replaced, false on failure
bool mission_replace_cmd(uint16_t index, const mission_command_t cmd);

/// get the ground course of the next navigation leg in centidegrees
/// from 0 36000. Return default_angle if next navigation
/// leg cannot be determined
int32_t mission_get_next_ground_course_cd(int32_t default_angle);

// returns false on any issue at all.
bool mission_set_item(uint16_t index, mavlink_mission_item_int_t* src_packet);
bool mission_get_item(uint16_t index, mavlink_mission_item_int_t* ret_packet);

/// get_current_nav_cmd - returns the current "navigation" command
const mission_command_t mission_get_current_nav_cmd();

// find the nearest landing sequence starting point (DO_LAND_START) and
// return its index.  Returns 0 if no appropriate DO_LAND_START point can
// be found.
uint16_t mission_get_landing_sequence_start();

/*
   find the nearest landing sequence starting point (DO_LAND_START) and
   switch to that mission item.  Returns false if no DO_LAND_START
   available.
 */
bool mission_jump_to_landing_sequence(void);

// jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort
bool mission_jump_to_abort_landing_sequence(void);

bool mission_contains_item(MAV_CMD command);

/// return true if the mission has a terrain relative item.  ~2200us for 530 items on H7
bool mission_contains_terrain_alt_items(void);

MAV_MISSION_RESULT mission_SanityCheckParams(const mavlink_mission_item_int_t* packet);

// mavlink_int_to_mission_cmd - converts mavlink message to an AP_Mission::Mission_Command object which can be stored to eeprom
//  return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
MAV_MISSION_RESULT mission_mavlink_int_to_mission_cmd(const mavlink_mission_item_int_t* packet, mission_command_t cmd);

MAV_MISSION_RESULT mission_convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t *packet,
        mavlink_mission_item_int_t *mav_cmd);

MAV_MISSION_RESULT mission_convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t *item_int,
        mavlink_mission_item_t *item);

// mavlink_cmd_long_to_mission_cmd - converts a mavlink cmd long to an AP_Mission::Mission_Command object which can be stored to eeprom
// return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure
MAV_MISSION_RESULT mission_mavlink_cmd_long_to_mission_cmd(const mavlink_command_long_t* packet, mission_command_t cmd);

// mission_cmd_to_mavlink_int - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS
//  return true on success, false on failure
bool mission_mission_cmd_to_mavlink_int(const mission_command_t cmd, mavlink_mission_item_int_t* packet);

const char *mission_command_type(const mission_command_t cmd);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



